//
// Created by 74354 on 2022/1/1.
//

#include "Motor.hpp"
#include "drv8301.h"
#include "commuTask.h"
#include "tim.h"
#include "math.h"


uint16_t Data_RegADC_M0[ADC_RegDATA_SIZE];
uint16_t Data_RegADC_M1[ADC_RegDATA_SIZE];

void DRV8301_t::setup()
{
    DRV8301_Obj *gate_driver = &this->gate_driver;
    DRV_SPI_8301_Vars_t *local_regs = &gate_driver_regs;
    DRV8301_enable(gate_driver);

    /* 设置drv8301的四个寄存器来初始化Drv8301 */
    DRV8301_setupSpi(gate_driver, local_regs);

    /*
            设置OCP_MODE(Programmable Overcurrent Protection)
            设置为锁存器关闭模式：
            如果过流，将关闭上下侧半桥的MOSFET，
            nFAULT引脚和nFAULT状态位将锁住，直到通过GATE_RESET位或快速EN_GATE复位脉冲接收到重置。
    */
    local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown;

    /*
            设置OC_ADJ_SET：Overcurrent Adjustment（5个位：32种模式）
            原理：MOSFET的 Vds 和 Id 正相关，Drv8301有一个 Vds 的监视器,用于监视Vds
                        当Vds > OC_ADJ_SET 时：说明过流。
            通过设置 OC_ADJ_SET 检测是否过流，当然****这个值需要根据具体的MOSFET具体设置
            公式：Overcurrent Trip = OC_ADJ_SET / MOSFET R DS(on)
            我这里的MOSFET是：HSBA4052 R DS(on):typical: 10.5 mOhm   max:15 mOhm
            取 30 A 和 10.5 mOhm
            计算得到： OC_ADJ_SET < 0.315  取为：DRV8301_VdsLevel_0p282_V
    */
    local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p282_V;

    // 20V/V on 500uOhm gives a range of +/- 150A
    // 40V/V on 500uOhm gives a range of +/- 75A
    // 20V/V on 666uOhm gives a range of +/- 110A
    // 40V/V on 666uOhm gives a range of +/- 55A
    switch (gain_param) {
        case gain10VpV:local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_10VpV;break;
        case gain20VpV:local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV;break;
        case gain40VpV:local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV;break;
        case gain80VpV:local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_80VpV;break;
        default:
            break;
    }

    local_regs->SndCmd = true;
    DRV8301_writeData(gate_driver, local_regs);
    local_regs->RcvCmd = true;
    DRV8301_readData(gate_driver, local_regs);

}

void Motor_t::calibrate_encoder() {

    float ang = 0;
    uint16_t  temp;
    int count = encoder->getCircleCount();
    while(true)
    {
        if(encoder->zPhaseAround)break;
        foc_controller->runToAngle(ang);
        ang += 5;
        osDelay(5);
    }
    encoder->zPhaseAround = false;
    osDelay(500);
    foc_controller->runToAngle(0);

    encoder->setCount(0);
    encoder->setCircleCount(0);
    is_calibrated_encoder = true;
}

void Motor_t::calibrate_motor() {

    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_1,0);
    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_2,0);
    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_3,0);
    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_4,10);
    osDelay(500);
    int total[3] = {0,0,0};
    for(int i = 0;i < 30;i++)
    {
        total[0] = total[0] + iABC_dig[0];
        total[1] = total[1] + iABC_dig[1];
        total[2] = total[2] + iABC_dig[2];
        osDelay(50);
    }
    i_phase_offset[0] = total[0]/30;
    i_phase_offset[1] = total[1]/30;
    i_phase_offset[2] = total[2]/30;

//    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_1,foc_controller->getTs());
//    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_2,0);
//    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_3,0);
//    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_4,foc_controller->getTs()/2);
//    osDelay(2000);
//
//    compute_Current();

//
//    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_1,0);
//    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_2,0);
//    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_3,0);

    is_calibrated_motor = true;
}


void Motor_t::compute_Current() {
    // 关闭TIM4 防止触发数据变换
    uint16_t a =  __HAL_TIM_GetCompare(&foc_controller->driver_timer,TIM_CHANNEL_4);
    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_4,0);

    iABC_ANO[0] = (iABC_dig[0]) * SCALE_Tran / (SAMPLE_REG * driver->v_Gain);
    iABC_ANO[1] = (iABC_dig[1]) * SCALE_Tran / (SAMPLE_REG * driver->v_Gain);
    iABC_ANO[2] = (iABC_dig[2]) * SCALE_Tran / (SAMPLE_REG * driver->v_Gain);

    __HAL_TIM_SET_COMPARE(&foc_controller->driver_timer,TIM_CHANNEL_4,a);
}

inline float Motor_t::getMachineRad() {return 2*PI*(encoder->getCircleCount() + encoder->getCount()/encoder->cpr);}


void Drv8301PrintInf(DRV_SPI_8301_Vars_t *regs)
{
    printf("Stat_Reg_1\n");
    printf("	FAULT: %d\n",regs->Stat_Reg_1.FAULT);
    printf("	FETHA_OC: %d\n",regs->Stat_Reg_1.FETHA_OC);
    printf("	FETHB_OC: %d\n",regs->Stat_Reg_1.FETHB_OC);
    printf("	FETHC_OC: %d\n",regs->Stat_Reg_1.FETHC_OC);
    printf("	FETLA_OC: %d\n",regs->Stat_Reg_1.FETLA_OC);
    printf("	FETLB_OC: %d\n",regs->Stat_Reg_1.FETLB_OC);
    printf("	FETLC_OC: %d\n",regs->Stat_Reg_1.FETLC_OC);
    printf("	OTSD: %d\n",regs->Stat_Reg_1.OTSD);
    printf("	OTW: %d\n",regs->Stat_Reg_1.OTW);
    printf("	PVDD_UV: %d\n",regs->Stat_Reg_1.PVDD_UV);
    printf("\n");

    printf("Stat_Reg_2\n");
    printf("	DeviceID: %d\n",regs->Stat_Reg_2.DeviceID);
    printf("	GVDD_OV: %d\n",regs->Stat_Reg_2.GVDD_OV);
    printf("\n");

    printf("Ctrl_Reg_1\n");
    printf("	DRV8301_CURRENT: %d\n",regs->Ctrl_Reg_1.DRV8301_CURRENT);
    printf("	DRV8301_RESET: %d\n",(regs->Ctrl_Reg_1.DRV8301_RESET)>>2);
    printf("	OC_ADJ_SET: %d\n",(regs->Ctrl_Reg_1.OC_ADJ_SET)>>6);
    printf("	OC_MODE: %d\n",(regs->Ctrl_Reg_1.OC_MODE)>>4);
    printf("	PWM_MODE: %d\n",(regs->Ctrl_Reg_1.PWM_MODE)>>3);
    printf("\n");

    printf("Ctrl_Reg_2\n");
    printf("	DC_CAL_CH1p2: %d\n",(regs->Ctrl_Reg_2.DC_CAL_CH1p2)>>4);
    printf("	GAIN: %d\n",(regs->Ctrl_Reg_2.GAIN)>>2);
    printf("	OCTW_SET: %d\n",regs->Ctrl_Reg_2.OCTW_SET);
    printf("	OC_TOFF: %d\n",(regs->Ctrl_Reg_2.OC_TOFF)>>6);
    printf("\n");


}
